/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-09-17 16:44:39
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-10-14 21:26:15
 */

#include <Arduino.h>
#include "RobotCtrl.hpp"
#include "PS2X_lib.h"
#include <U8g2lib.h>
#include "DisplayBMP.h"


/* 手柄引脚定义 */
#define PS2_DAT     26
#define PS2_CMD     25
#define PS2_SEL     33
#define PS2_CLK     32
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false
/* 灯控制引脚定义*/
#define LED_PIN     2
/* 蜂鸣器引脚定义*/
#define BUZZER_PIN  27
/* 电池电压ADC引脚定义 */
#define BAT_PIN     34
#define ADC_EXAMPLE_CALI_SCHEME     ESP_ADC_CAL_VAL_EFUSE_TP
#define ADC_EXAMPLE_ATTEN           ADC_ATTEN_DB_11
/* OLED引脚定义 */
#define OLED_SDA    21
#define OLED_SCL    22

/* 定义机PS2X手柄对象指针 */
PS2X *ps2x;
bool PS2Connect = false;
bool PS2Contorl = false;

/* 定义机器人 */
RobotCtrl *Robot;
#define BuffSize    50
bool ReadServoFlag = false;
uint16_t ServoDataLen = 0;
uint8_t ServoDataBuff[BuffSize] = {0};


/* 定义OLED对象 */
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);


void Task_RobotCtrl(void *pvParameters);
void Task_Display(void *pvParameters);


void setup()
{
    Serial.begin(115200);
    Serial2.begin(115200);
    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, LOW);
    pinMode(BUZZER_PIN, OUTPUT);
    digitalWrite(BUZZER_PIN, HIGH);
    analogSetAttenuation(ADC_11db);
    delay(500);

    /* 创建机器人控制协程：协程函数，协程名，栈空间大小，协程函数参数，优先级，协程句柄 */
    xTaskCreate(Task_RobotCtrl, "Task_RobotCtrl", 4096, NULL, 3, NULL);
    /* 创建OLED显示协程：协程函数，协程名，栈空间大小，协程函数参数，优先级，协程句柄 */
    xTaskCreate(Task_Display, "Task_Display", 4096, NULL, 2, NULL);

    Serial2.println(F("Init"));
    digitalWrite(BUZZER_PIN, LOW);
}


void loop()
{
    // while (1)
    // {
    //     delay(500);
    //     Serial2.println(F("Move"));
    //     Robot->Robot_Forward();

    //     // Robot->RobotSerialServoMove(1, 300, 100);   // 往前
    //     // Robot->RobotSerialServoMove(3, 300, 100);   // 往前
    //     // Robot->RobotSerialServoMove(5, 700, 100);   // 往前
    //     // Robot->RobotSerialServoMove(7, 700, 100);   // 往前
    // }

    // // 读取虚拟串口接收的数据
    // while (SoftUart.available() > 0)
    // {
    //     ServoDataBuff[ServoDataLen++] = SoftUart.read();
    //     ReadServoFlag = true;
    //     delay(1);
    // }
    // // 打印虚拟串口接收的数据
    // if (ReadServoFlag)
    // {
    //     ReadServoFlag = false;
    //     Serial2.print("Read Servo Len: "); Serial2.println(ServoDataLen);
    //     Serial2.print("Read Servo Data: "); Serial2.write(ServoDataBuff, ServoDataLen);
    //     ServoDataLen = 0;
    // }
}


/**
 * @function: MyDelay_ms
 * @brief: 阻塞延迟毫秒函数
 * @param {uint32_t} ms：延迟的毫秒值
 * @return {*}
 */
void MyDelay_ms(uint32_t ms)
{
    uint32_t lastTime = millis();
    uint32_t afterTime = 0;
    while(1)
    {
        afterTime = millis();
        if(afterTime >= lastTime && (afterTime - lastTime >= ms))
        {
            return;
        }
        else if((afterTime < lastTime) && (0xFFFFFFFF - lastTime + afterTime > ms))
        {
            return;
        }
    }
}


/**
 * @function: Task_RobotCtrl
 * @brief: 机器人控制协程，每50ms检测一次PS2X手柄的键值，并执行对应的操作
 * @param {void} *pvParameters
 * @return {*}
 */
void Task_RobotCtrl(void *pvParameters)
{
    /* 定义PS2X遥控手柄 */
    uint16_t buttonCnt = 0;     // 防止短按一次出现执行两次的情况
    int error = 0;      // 初始化错误类型
    byte type = 0;      // 手柄类型
    byte vibrate = 0;

    /* 初始化机器人对象、手柄对象 */
    Robot = new RobotCtrl(Serial);
    ps2x = new PS2X();
    vTaskDelay(100 / portTICK_PERIOD_MS);

    /*-----------------------------------> PS2X初始化 BEGIN <----------------------------------------------*/
    error = ps2x->config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

    if (error == 0)
    {
        Serial2.print("Found Controller, configured successful ");
        Serial2.print("pressures = ");
        if (pressures)
            Serial2.println("true ");
        else
            Serial2.println("false");
        Serial2.print("rumble = ");
        if (rumble)
            Serial2.println("true)");
        else
            Serial2.println("false");
        Serial2.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
        Serial2.println("holding L1 or R1 will print out the analog stick values.");
        Serial2.println("Note: Go to www.billporter.info for updates and to report bugs.");
    }
    else if (error == 1)
        Serial2.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

    else if (error == 2)
        Serial2.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

    else if (error == 3)
        Serial2.println("Controller refusing to enter Pressures mode, may not support it. ");

    type = ps2x->readType();
    switch (type)
    {
    case 0:
        Serial2.println("Unknown Controller type found ");
        break;
    case 1:
        Serial2.println("DualShock Controller found ");
        break;
    case 2:
        Serial2.println("GuitarHero Controller found ");
        break;
    case 3:
        Serial2.println("Wireless Sony DualShock Controller found ");
        break;
    }
    /*-----------------------------------> PS2X初始化 END <----------------------------------------------*/

    // 机器人复位
    Serial2.println(F("Roset"));
    Robot->RobotReset();
    vTaskDelay(1000 / portTICK_PERIOD_MS);

    for (;;)
    {
        PS2Connect = ps2x->read_gamepad(false, vibrate); // read controller and set large motor to spin at 'vibrate' speed

        if (ps2x->Button(PSB_START)) // will be TRUE as long as button is pressed
            Serial2.println("Start is being held");
        if (ps2x->Button(PSB_SELECT))
            Serial2.println("Select is being held");

        if (ps2x->Button(PSB_PAD_UP))
        { // will be TRUE as long as button is pressed
            if(buttonCnt++%3 == 0)
            {
                PS2Contorl = true;
                Serial2.print("Up held this hard: ");
                Serial2.println(ps2x->Analog(PSAB_PAD_UP), DEC);
                Robot->Robot_DollyMoves(200);
            }
        }

        if (ps2x->Button(PSB_PAD_RIGHT))
        {
            if(buttonCnt++%3 == 0)
            {
                PS2Contorl = true;
                buttonCnt = PSB_PAD_RIGHT;
                Serial2.print("Right held this hard: ");
                Serial2.println(ps2x->Analog(PSAB_PAD_RIGHT), DEC);
                Robot->Robot_TuruACorner(100);
            }
        }

        if (ps2x->Button(PSB_PAD_LEFT))
        {
            if(buttonCnt++%3 == 0)
            {
                PS2Contorl = true;
                buttonCnt = PSB_PAD_LEFT;
                Serial2.print("LEFT held this hard: ");
                Serial2.println(ps2x->Analog(PSAB_PAD_LEFT), DEC);
                Robot->Robot_TuruACorner(-100);
            }
        }

        if (ps2x->Button(PSB_PAD_DOWN))
        {
            if(buttonCnt++%3 == 0)
            {
                PS2Contorl = true;
                buttonCnt = PSB_PAD_DOWN;
                Serial2.print("DOWN held this hard: ");
                Serial2.println(ps2x->Analog(PSAB_PAD_DOWN), DEC);
                Robot->Robot_DollyMoves(-200);
            }
        }

        vibrate = ps2x->Analog(PSAB_CROSS); // this will set the large motor vibrate speed based on how hard you press the blue (X) button
        if (ps2x->NewButtonState())
        { // will be TRUE if any button changes state (on to off, or off to on)
            if (ps2x->Button(PSB_L3))
                Serial2.println("L3 pressed");
            if (ps2x->Button(PSB_R3))
                Serial2.println("R3 pressed");
            if (ps2x->Button(PSB_L2))
                Serial2.println("L2 pressed");
            if (ps2x->Button(PSB_R2))
                Serial2.println("R2 pressed");
            if (ps2x->Button(PSB_TRIANGLE))
            {
                Serial2.println("Triangle pressed");
                if(digitalRead(BUZZER_PIN))
                {
                    digitalWrite(BUZZER_PIN, LOW);
                }
                else
                {
                    digitalWrite(BUZZER_PIN, HIGH);
                }
            }
        }

        if (ps2x->ButtonPressed(PSB_CIRCLE)) // will be TRUE if button was JUST pressed
        {
            Serial2.println("Circle just pressed");
            if(digitalRead(LED_PIN))
            {
                digitalWrite(LED_PIN, LOW);
            }
            else
            {
                digitalWrite(LED_PIN, HIGH);
            }
        }
        if (ps2x->NewButtonState(PSB_CROSS)) // will be TRUE if button was JUST pressed OR released
            Serial2.println("X just changed");
        if (ps2x->ButtonReleased(PSB_SQUARE)) // will be TRUE if button was JUST released
            Serial2.println("Square just released");

        if (ps2x->Button(PSB_L1) || ps2x->Button(PSB_R1))
        { // print stick values if either is TRUE
            Serial2.print("Stick Values:");
            Serial2.print(ps2x->Analog(PSS_LY), DEC); // Left stick, Y axis. Other options: LX, RY, RX
            Serial2.print(",");
            Serial2.print(ps2x->Analog(PSS_LX), DEC);
            Serial2.print(",");
            Serial2.print(ps2x->Analog(PSS_RY), DEC);
            Serial2.print(",");
            Serial2.println(ps2x->Analog(PSS_RX), DEC);
        }

        vTaskDelay(50 / portTICK_PERIOD_MS);
        PS2Contorl = false;
        // MyDelay_ms(50);
    }

    vTaskDelete(NULL);
}


/**
 * @function: Task_Display
 * @brief: 显示屏显示协程
 * @param {void} *pvParameters
 * @return {*}
 */
void Task_Display(void *pvParameters)
{
    /* init */
    uint8_t DisplayBLBL = 1;
    /* OLED初始化 */
    u8g2.begin();
    vTaskDelay(100 / portTICK_PERIOD_MS);
    u8g2.clear();
    u8g2.setFont(u8g2_font_unifont_t_symbols); // 设置字体

    for(;;)
    {
        Serial2.println("Display Task!");
        u8g2.firstPage();   // 设置当前页码位置变成0

        if(1)   // 打印LED灯和蜂鸣器状态
        {
            do
            {
                if(digitalRead(LED_PIN))
                {
                    u8g2.drawXBM(0, 0, 64, 64, LED_ON_BMP_64X64);
                }
                else
                {
                    u8g2.drawXBM(0, 0, 64, 64, LED_OFF_BMP_64X64);
                }

                if(1)
                {
                    switch (DisplayBLBL)
                    {
                    case 1:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_1_BMP_64X64);
                        break;
                    case 2:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_2_BMP_64X64);
                        break;
                    case 3:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_3_BMP_64X64);
                        break;
                    case 4:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_4_BMP_64X64);
                        break;
                    case 5:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_5_BMP_64X64);
                        break;
                    case 6:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_6_BMP_64X64);
                        break;
                    case 7:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_7_BMP_64X64);
                        break;
                    case 8:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_8_BMP_64X64);
                        break;
                    case 9:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_9_BMP_64X64);
                        break;
                    case 10:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_10_BMP_64X64);
                        break;
                    case 11:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_11_BMP_64X64);
                        break;
                    case 12:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_12_BMP_64X64);
                        break;
                    case 13:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_13_BMP_64X64);
                        break;
                    case 14:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_14_BMP_64X64);
                        break;
                    case 15:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_15_BMP_64X64);
                        break;
                    case 16:
                        u8g2.drawXBM(64, 0, 64, 63, BLBL_16_BMP_64X64);
                        break;

                    default:
                        break;
                    }
                }
                else
                {
                    u8g2.drawXBM(64, 0, 64, 63, BLBL_1_BMP_64X64);
                }
                // u8g2.setCursor(64, 15);
                // u8g2.print("PS2: ");
                // u8g2.print((PS2Connect) ? ("ON") : ("OFF"));
                // u8g2.setCursor(0, 15);
                // u8g2.print("LED: ");
                // u8g2.print((digitalRead(LED_PIN)) ? ("ON") : ("OFF"));
                // u8g2.setCursor(0, 30);
                // u8g2.print("BUZ: ");
                // u8g2.print((digitalRead(BUZZER_PIN)) ? ("ON") : ("OFF"));
                // u8g2.setCursor(0, 45);
                // u8g2.print("BAT: ");
                // u8g2.print(analogRead(BAT_PIN));
                // u8g2.setCursor(64, 15);
                // u8g2.print(millis() / 1000);    // 打印当前开机时间，单位s
            }while(u8g2.nextPage());    // 等待绘制完成
            if(DisplayBLBL++ >= 16)
            {
                DisplayBLBL = 1;
            }
        }

        vTaskDelay(50 / portTICK_PERIOD_MS);
    }
    vTaskDelete(NULL);
}





